/***************************************************************************************************************************
* command_to_drone.h
*
* Author: SFL
*
* Update Time: 2020.9.13
*
* Introduction: Drone control command send  to fast drone using serial
                与串口相关的接口包，具体怎么写需要向杨司臣请教
***************************************************************************************************************************/


#ifndef SERIAL_TO_DRONE_H
#define SERIAL_TO_DRONE_H


#include <ros/ros.h>
//串口头文件
#include <serial/serial.h>


using namespace std;
serial::Serial ser;
// uint8_t def_Msg[32]= {0x0a, 0x0a, 0x05, 0xdc, 0x05, 0xdc, 0x03, 0xea,  0x05, 0xdc, 0x06, 0xdc, 0x06, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc,};
uint8_t radioMsg[25]={0x00};
int channels[16] = {0x00};

int serial_init();
// uint16_t radio_map(int pass_num , float pos_value);
// ros::NodeHandle nh("~");
// string port_name;
// long int bandrate;
// float time_out;

int serial_init()
{
    try 
    {
        ser.setPort("/dev/ttyUSB0");
        ser.setBaudrate(100000);
        ser.setParity(serial::parity_even);
        ser.setStopbits(serial::stopbits_two);
        ser.setBytesize(serial::eightbits);
        ser.setFlowcontrol(serial::flowcontrol_none);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    } 
    catch (serial::IOException &e) 
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return 0;
    }
 
  if (ser.isOpen())
   {
        ROS_INFO_STREAM("Serial Port initialized");
   } 
   else 
   {
       ROS_INFO_STREAM("Serial Port not open");
       return 0;
   }
   return 1;
}

#endif //SERIAL_TO_DRONE_H

